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INTRODUCTION 


The objective initially proposed was to analyze RMS performance data collected 
during a Shuttle Flight. The data was to consist of video TRAC data collected via 
a video recorder. Unfortunately, the flight never collected the data due to higher 
priority experiments superseding it. As a result, the research team at Texas A&M 
was directed to work on several other pressing issues regarding the TRAC sensor. All 
but one of these issues have been reported to the contract monitor (Mr. Leo Monford) 
earlier in the form of periodic status reports. 

In fulfilment of the grant conditions, the last issue investigated is being reported 
as the final report. Ordinarily, a TRAC sensor determines the orientation of an object 
by analyzing the image reflected from a mirror target. The concern addressed by this 
report is to develop a method for using the TRAC sensor when the target does not 
reflect a usable image. 
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Determination of Target Pose without Sensor Reflection 


Objective 

Given two objects or structures with relative pose between them, an object with an integral, 
specially configured target and an object with a vision sensor, determine the position and orientation 
of the target relative to the sensor. No mirror reflection of the sensor off the target and back to the 
sensor is available. 

Hardware 

The target is a configuration of four distinct objects (LED arrays, retro-reflectors, etc.) that can 
be independently recognized. The vision system sensor views the target and a computer algorithm 
determines the azimuth and elevation of each LED array relative to the sensor coordinate system. 

Coordinates 

In Figure 1 the visipn system is at the origin, O, of the right-hand coordinate system fixed to 
the sensor object. The target is at P. The forward axis of the sensor, which is the optical axis of the 
vision system, is in the z direction. Upward is the y direction and to the left is the x direction. The 
position vector from O to P is R which has length r. Azimuth angle, 0, is defined about the y 
axis measured from positive z; elevation angle, a, is about x in the negative direction measured 
from the x-z plane. 
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A working angle cp is defined as the angle between the position vector and the x-z plane. 

Cartesian geometry shows that if 

R = pJ + PyJ + Pzk 0) 

then 

p x = r coscp sin0 
Py ■ r sincp 

Pj = r coscp cos0 ^ 2 ) 

where 

cp « tan' 1 £ — J » tan' 1 £ = tan' 1 [tana cos0 ] 


Full geometry 

The entire geometry including the 4 target objects is shown in Figure 2. 



Figure 2 - Geometry of tracking scenario 

The origin of the x-y-z system is the vision system, the 4 black dots are the target objects P t , the R i 
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are the radius vectors between the sensor object and the individual target objects, and the V (J are the 
vectors connecting the points in the target. The azimuth and elevation of each radius vector is 
known, but its length is not. The geometry of the target is known, that is the lengths of the V y and 
the angles between the V- . 

Determination of target pose 

If the lengths of the R, (r ( ) can be found, the points Pj can be calculated and the target pose will 
be known. To the investigators’ knowledge, any solution to this problem involves the simultaneous 
solution of a set of nonlinear algebraic equations. To solve this set of equations, an initial guess of 
the rj converges to a solution by iteration. 

Two approaches are taken to this problem, both of which are detailed in the next section of this 
paper. The “direct” approach takes a guess at one of the radius vector lengths, r v and calculates the 
remaining radius vectors based on geometry. If these vectors determine a target configuration 
geometry that agrees with the actual geometry to some tolerance, a solution is found, otherwise a 
new guess at rj is taken. The second approach guesses all four radius vectors, applies the geometric 
nonlinear relations, f,(r v r 2 , r 3 , r^ = f^r) = e, and calculates a residual that indicates error. The four 
vector lengths are simultaneously altered with a Newton-Raphson technique for sets of nonlinear 
equations. 

Direct approach 

An initial guess of one of the radius vector lengths, r ( , allows the calculation of a second radius 
vector length, rj from the geometry of Figure 3. 



Figure 3 - Guess at radius vectors 
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The law of cosines is applied with a guess for r t , and a known v- and p. P is the angle subtended by 
Rj and Rj. The cosine of P is found as the dot product of the radius unit vectors, • e^. 

The geometry yields zero, one, or two real rj for each given r 4 as seen in the quadratic equation 
of equation 4. 


if -^cosPrj+rf-Vij = 0 (4) 

In Figure 4, four possible cases, A-D, are shown for the solution of equation 4. Rj and Rj are two 
radius vectors of known directions but unknown lengths and V y is the target vector between R ; and 
Rj with known length but unknown direction. The circles all have a radius of Vy = |Vy|. a-d are 
distances along R, from the origin, O. 




Case A shows a guessed length for Rj of a. No direction of V (J allows contact with Rj so the 
guessed length is too long and the solution to equation 4 would yield complex numbers. Case B 
shows a guess length for R x of b that allows one solution, vector V y is normal to Rj. The quadratic 
equation would result in a repeated root Case C shows a length for R ( of c that gives two solutions 
to the direction of Vy and Case D shows that one of the two solutions implies a negative Rj which is 
not physically possible in the targeting scenario. 

For the solution of all four radius vectors, the following steps are taken which will be detailed 
afterwards: 
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a) Determine valid range and increment for r j. 

b) Cycle through the r j range from maximum to minimum. 

1) For each r j calculate possible r 2 , r 3 , and r 4 . 

2) For each combination of r j ... r 4 determine all v - from equation 4. 

3) Calculate error as difference between v- from step b2 and actual target v- . 

4) Record r min that minimizes error 

5) Pick new r } and repeat b. 

c) Determine new range near r l ,,^5 repeat step b 

d) Stop when rj increment is less than tolerance. 

The maximum r t value is determined such that given v~, real r 2 , r 3 , and r 4 exist. Case B in 
Figure 4 shows the maximum r i that allows a real solution for rj. If r t >b, there is no solution. The 
result is that 

r . - JSL 

i max s [ n |3 (5) 

This maximum is determined for each of the 3 vectors r 2 -r 4 and the minimum of these is retained. 

The minimum Tj is initially taken to be zero and its range is arbitrarily divided into n 
increments of length r^ = r ( mitX /n. r j is cycled through this range from maximum to minimum and 
for each r j the r 2 -r 4 are determined with equation 4. Since there are usually 2 rj ( j=2,3,4) for each 
rj, there are 2 3 combinations of radius vectors for each r 

Each of these combinations of r ( , rj is inserted into equation 4 for i,j=2,3; 2,4; 3,4 and the v- 
are calculated. The closer these calculated v- are to the actual v- the better the estimate of r. The 
difference between the two v- is termed “error.” For the r , that gives a minimum error in the current 
range ( r j*) , a new range for r t is chosen to be r j* + r ^ to r t * - r^. The maximum value is kept no 
more than Rj max and the minimum value is never less than zero. A new increment is chosen by 
dividing the new range into n divisions. When the r inc is less than some tolerance and the minimum 
error is found in the current range, the solution is in hand. 

The FORTRAN code that implements this strategy is in Appendix A. 


Newton- Raphson approach 

This method solves a set of 4 nonlinear equations where the (3- and v- are known and the r^ are 
unknown. Equation (6) is the set of equations; with correct r, the fj = 0. 
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fi(ri, T 2 , r 3 , r 4 ) = x\ + r\ - lx x x 2 cosP 12 ~ v 2 2 
f 2 (ri, r 2 , r 3 , r 4 ) = r 2 + r 2 - 2r 2 r 3 cosp 23 - v 2 2 3 
f 3 (r l5 x 2 , r 3 , r 4 ) - r 2 + rj - 2r 3 r 4 cos0 34 - v 3 2 4 
ftOj* r 2 , r 3 , r 4 ) = rj + r 2 - 2r 4 r t cos0 41 - v 41 


Press et al. [1986] outline the method where a truncated Taylor series expansion approximates the fj 
with the second and higher order terms dropped. 


f i 0; + 6r) = f i (r) + 




(7) 


For an initial guess of r, the left had sides of equation (6) are not usually zero. From equation (7), 

the 6r are required that make fj (r + 5r) zero given an initial fj (r). Equation (7) is a linear set of 

equations in 5r that can be solved with LU decomposition or any other method. As long as the 

initial guess of r is close enough to the solution, the Newton-Raphson schemes converges nicely. 

£ 

Otherwise, the scheme may never converge or perhaps converge to the wrong solution. 

In practice, the algorithm converges very well in a local neighborhood around the correct 
solution. If the guess is not in the neighborhood the scheme either does not converge or it converges 
to a wrong solution. The difficulty is in defining the neighborhood. In testing, sometimes an initial 
guess for z of 50% of the solution would converge in less than 50 iterations. Other times, an initial 
guess within 5% of the solution would never converge. This uncertainty leaves the Newton-Raphson 
lacking without further inquiry. 

The FORTRAN code for this scheme is in Appendix B. 


References 

Press, W. H., Flannery, B. P., Teukolsky, S A., and Vetterling, W. T., 1986, Numerical 
Recipes: The An of Scientific Computing, Cambridge University Press, Cambridge, pp. 269. 
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Appendix A - FORTRAN code for direct approach 


j,******************************************************************************* 

c target . for 


program target 


c 



c 

c Routine find Ri for 4 vectors by guessing RI, calculating R2 , R3, and R4 
c and applying law of cosines to find residual errors, Ri that minimizes 
c errors is best guess. 


cl 2 3 4 5 6 7 

C2345678 901 234 567 890 1 23 456 789 0 1 2 345 678 90 1 234 567 890 1 23 456 789 0 1 2 345 678 90 1 2 


real r2(2),r3(2), r4(2), err(2,2) 
real t(4,4), az(4), el(4), phi(4), 
rea I merit, mini, min2 
open (unit*10, f I I e = ' t ar get . i n p 1 , 


c read input data from file TRRGET.INP 

read (10,*) toldrt, t o 1 ro in 
tinax * 0 
do 5, ! ■ 1 , 3 

do 5 , J ■ i + 1 , 4 
read (10,*) t ( i , j ) 
t ( j , I ) * t ( i , j ) 

5 tmax ■ amaxl (tmax, 1 ( i , j ) ) 
read (10,*) (az ( i ) , i ■ 1 , 4) 
read (10,*) (el ( i ) , i ■ 1 , 4) 


c (4, 4) , s2 (4, 4) 
s t at u s= ' o I d 1 ) 


! Conuerg tolerances 
! Initialize max value 


I S ymm et r i c 
! riax target vector 
! Rz i muth angles 
! Elevation ang I es 


c convert to radians from degrees 

pi *3,14159 ! Calculate pi 

do 10 i*1 , 4 

az(i) * az(i) * pi/180 
e I ( i ) * e I ( i ) * pi/180 

10 phi(i) 12 atan( tan(el(i)) * cos(az( i )) ) 


c calculate cos and sin of angles between radius vectors (cos 


el dot e2) 


do 20 , 1*1, 3 

do 20, j * I +1 , 4 

c ( i , j ) ■ sin(phi(i)) * sin(ph i (j )) + 

* cos(ph i ( i ) ) *cos(az( i ) ) * cost ph i ( j ) )*cos( az( j ) ) + 

* cos(ph i ( i ) ) *s I n(az( i ) ) * c os ( ph I ( j ) )*s i n( az( j ) ) 
c( j , i ) * c( i , j ) 

erite(6,*) ‘c,t i,j, c(i,j), t(i,j) ! echo input 

s2(i,j) * 1 - c(i,j)**2 ! sine squared of angle 

20 s2( j , i ) - s2 ( i , j ) ! Symmetric 

c limits on r i values 


rma x * 1 . e20 
do 30, j*2, 4 

tap * sqrt ( s2( 1 , j ) ) 

if (tap . eq . 0) top*1 . e-20 
tap * t ( 1 , j )/t mp 


initial rm ax 

sine of angle 1-i 

no / by zero 

max ri due to r ( j ) 
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if (trap .eq, 0) t m p * 1 . e-20 
t mp - t ( 1 , j )/tmp 
r max ■ ani nl (raaxj trap) 

30 con t i nue 


no / by zero 

max rl due to r ( j ) 

max r 1 is min of 1 


c loop to find radius vector lengths - set increment based on limits 

pm J n ■ 0 . ! r 1 mini mum 

rup * rmax 

drl * rraax / 10 ! rl increment 

mini - 1 . e20 ! initial minimums 

m i n 2 * 1 . e20 
35 rar i t e ( 6 , * ) ’ * 

uirite(6,*) 'dr1-> drl, ' rup-> 1 , rup , ! counter 

* ' r m i n- > ' , r m i n 

do 80, rl « rup, rmln, -drl ! loop rls 

c t®o values of r2,r3,and r4 for each rl 

t mp 2 - t ( 1 , 2 ) ** 2 - r 1 **2 * s2(1,2) 
if (tmp2 .It. 0.) tmp2 ■ 0. 
t mp3 = t( 1 ,3)**2 - rl **2 * s2( 1 ,3) 
i f (tmp3 .It. 0.) tmp3 * 0. 

r2(1) * rl * c ( 1 4 2 ) + sqrt(tmp2) 

p 2 ( 2 ) * rl * c(1,2) - sqrt(tmp2) 

r 3 ( 1 ) - rl * c(1 ,3) + sqrt(t mp3) 

r3(2) M rl * c(1,3) - sqrt(tmp3) 


c check 2-3 vector, 4 combinations 


do 60, i *1 .$,2 
do 60 j-1,2 

err ( i , j ) » r 2 ( i )**2 + r3(j )**2 - 
« 2*r2( i )*r3( j )*c(2,3) - t(2,3)**2 


t mp 1 - t ( 1 , 4 ) ** 2 - r 1 **2 * s2(1,4) 
I f (trap4 .It, 0.) t mp4 = 0. 
r4(1) = rl * c(1,4) + sqrt(tmp4) 
r4(2) * rl * c(1,4) - sqrt(tmp4) 


* 


n 


do 55 k-1 ,2 

err24 - r2(l)**2 + r4(k)**2 - 
2*r2( i )*r4(k)*c(2,4) - 
err 34 * r3(j)**2 + r4(k)**2 - 
2*r3( j )*r4(k)*c(3,4) - 


! vector 2-4 

t ( 2 , 4 ) **2 

! v ect or 3-4 

t (3,4)**2 


c merit is sum of absolute 2-3, 2-4, and 3-4 vector errors 


merit = abs(err24) + abs(err34) + abs (er r ( i , j ) ) 


( raer i t 

.It. mini) t hen 


m i n2 

= 

mini 

! save best 

rl f 2 

■ 

r 1 f 1 


r2 f 2 


r 2 f 1 


r3 f 2 


r 3 f 1 


r 4 f 2 

E 

r 4 f 1 


mini 

» 

merit 


rl f 1 

= 

r 1 


r2f1 

- 

r 2 ( i ) -* 


r 3 f 1 

= 

r 3 { j ) 


r 4 f 1 

* 

r 4(k) 


se 





8 




if (merit .it. 

m 1 n2) 

then 


m i n2 * merit 
r 1 f 2 - rl 
r2 f 2 * r 2 ( i ) 
r3f 2 * r3 ( j ) 
r4f2 « M(k) 




en d i f 
end i f 



55 

con t i nue 



60 

co nt i nue 



60 

con t i n ue 




wr i t e( 6 , * ) ' 1-> ' , r 1 f 1 , 

r2 f 1 , 

r3f1, rl f 1 


»r i te( 6, * ) 1 mer it 1 -> 1 j 

mini 



■ rl 16(6,*) ' 2-> *, rl f2, 

r2 f 2 , 

r3 f 2 j Mf2 


wr 1 t e ( 6 j * ) ' mer it 2- > 1 , 

m i n2 



if ( dr 1 . 1 e . t o 1 dr 1 . or . 

mini 

. 1 e . t o ! m i n) 


rup ■ amaxl ( r 1 f 1 , r1f2) 

+ drl 



if (rup . gt . rmax) rup^rmax 



rm i n ■ am ini (rl f 1 , r 1 f 2 ) 
dr 1 * (rup-rin i n ) / 1 0 . 
go to 35 

- drl 



end 
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saue second best 


! output 

! within t o I ? 

! new max 

! new min 

! new increment 

! new range & increment 


9 



Appendix B - FORTRAN code for Newton-Raphson approach 
(Code in all CAPS is from Press [1986]) 


C 

C 

c 

c 


ROOT .FOR 


program root 


c Program to find radius vector lengths for comet geometry 
real r(4), az(4), el(4), phi(4) 

include 'root. Inc' ! Common for "usrfun" 

c coaion /d i */ t(4,4), 0(4,4), s(4,4), betasu* 

c open i npu t file 

open (unit*10, f i I e s ' roo t . I np ' , s t at us* ' o I d 1 ) 

c read Input data from file ROOT.DRT 

read(10,*) tolx, tolf, ntrial 
do 5, i*1 , 3 

do 5 , j ■ i + 1 , 4 

read (10,*) t( I , J ) 

5 t ( j t i ) - t ( i , j ) 

read ( 10, *) (*az( i ) , i *1 , 4) 
read (10,*) (el(i), 1*1, 4) 

c conuert to radians from degrees 

pi - 3.14159 
do 10 1-1 , 4 

az ( i ) * az ( I ) * p i / 1 80 
e I ( I ) - el(i) * pi/180 
10 phi(l) ■ atan( tan(el(i)) * cos(az( i )) ) 

c calculate cos and sin of angles between radius vectors (cos * el dot e2) 


I Tolerance x & f , max. Iters. 


! Target vector lengths 
! Symmetric 
I Rz i mut h angles 
! E I e vat i on angles 


! Pi 


do 20, i * 1 , 3 
do 20, j - i + 1 , 4 

c(i,j) - s!n(phi(i)) * sln(phi(j)) + 

* cos(phi(i))*cos(az(i)) * cos(phi(j))*cos(az(j)) + 

* cos(ph I ( I ) ) *s i n(az( i ) ) * c os ( ph I ( j ) )*s i n( az ( j ) ) 


c ( j , I ) * c ( i , j ) 
s ( i , J ) = sqrt ( 1 - c ( I , j ) **2 
20 s ( j ,1) - s ( i , j ) 

c maximum radius vectors and initial r 


fac - 1.0 
22 do 30 1*1 , 4 

rmax * 1 . e20 
do 25 j*1, 4 

if ( j . eq . i ) go to 25 


Sym met r i c 
Sine 

Syramet r i c 
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large to start 
no ang I e her e 



! no limit here 


if ( 3 ( i , J ) . eq , 0 . ) go to 25 

test ■ t ( I , j ) / 3 ( * > j ) 
rmax - aminl (rmax, test) 

25 c ont i nu e 

r(i) * fac * rmax ! start at 80# max 

30 con t i nue 

write ( 6 , * ) 'Initial r ( i ) ' 
write (6,*) ( r ( i ) , 1=1, 4 ) 

c cal! 9 0 I u er 

nt = n t r i a I 

call mnewt(nt, r, 4, tolx, toif) 
if (nt . eq , 0) then 

f ac= fac - 0,1 

if (fac .eq. 0) stop I plum run out 

go to 22 
end i f 

c check results 

do 50 , i « 1 , 3 
do 50, j * i + 1 , 4 

to * sqrt( r(i)**2 + r(j)**2 - 2 *r ( i )* r ( j ) *c ( i , j ) ) 

50 wr i te (6, *) ' i n ' , t ( I , j ) , ' out 1 , t c 

write(6,*) ' Rs 1 , (r(l), i * 1 , 4) 

stop 

end 
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SUBROUTINE HNEUT(NTRIRL,X,N,TOLX,TOLF) 
PARRHETER (NP»4) 

DIHENSION X(NP),RLPHR(NP,NP), BETR(NP), INOX(NP) 
DO 13 K = 1 , NTR I RL 

CALL USRFUNCXjflLPHfi^BETR) 

ERRF-0 . 

DO 11 1 = 1 ,H 

ERRF*ERRF + RBS(BETfl( I ) ) 

11 CONTINUE 
IF(ERRF.LE.TOLF)RETURN 

CRLL LUDCnP( RLPHfl, N,NP, I NDX ,D) 

CALL LUBKSB(RLPHR,N,NP, I NDX , BETA) 

ERRX-0 . 

DO 12 1 = 1 ,N 

ERRX-ERRX + RBS (BETfl( I ) ) 

X ( I ) =X( I )+BETR( I ) 

12 CONTINUE 
IF(ERRX.LE.TOLX)RETURN 

13 CONTINUE 
RETURN 
END 
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SUBROUTINE LUDCf1P(fi,N,NP, I NDX , D) 

PRRflriETER (NI1fiX*1 00, T I H V= 1 .0E-20) 

DINENSION fl(NP,NP), INDX(N) ^UUlNflfiX) 

D» 1 . 

DO 12 I = 1 , N 
RRI1fiX-0. 

DO 11 J=1 ,N 

IF ( HBS (fl( l,J)).GT.RRf1RX) RfiNfiX =RB S(fl ( I , J) ) 

11 CONTINUE 

IF (flRNRX . EQ . 0 , ) PAUSE 'Singular matrix.' 

UU( l)=1 ,/RRNRX 

12 CONTINUE 

DO 19 J“ 1 , N 

IF (J. GT. 1 ) THEN 
DO M I =1 , J-l 
SUN=R( I , J) 

IF (I .GT. 1 ) THEN 
DO 13 K = 1 j 1 — 1 

sun=sun-R( i , k)*r(k, j) 

13 CONTINUE 

R(i ,j)=sun 

END I F 

14 CONTINUE 
END IF 
RRNRX=0 . 

DO 16 I = J j N 
SUn=R( I , J) 

IF (J.GT. 1 ) THEN 
DO 15 K=1 , J-1 

SUn>SUN-fl( I , K)*fl(K,J) 

15 CONTINUE* 
fl( I , j)=sun 

END I F 

DUN=UU( I )*RBS (SUN) 

IF ( DU11 . GE . RR IlflX ) THEN 

i n fl x = i 
RflnRX»Dun 
END I F 

16 CONTINUE 

IF (J. NE. I f1RX)THEN 
DO 17 K-1,N 
DUN=fi( inox, K) 

R ( mRX,K)«fl(J,K) 
fi( J, K )=DUM 

17 CONTINUE 
D =-D 

uu( i nflx ) »u u ( j ) 

END IF 

I ND X ( J ) * I n fl X 
IF(J,NE.N)THEN 

I F(fl( J, J) . EQ. 0. )fl(J , J)=T I NV 
DUH=1 ,/fl(J, J) 

DO 16 I = J+ 1 , N 

fl( I , J)=fl( I , J)*DUN 

18 CONTINUE 
END IF 

19 CONTINUE 

IF(fl(N,N) .EQ.O . )fi (N, N > - T I N V •"* 

RETURN 

END 
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SUBROUTINE LUBKSB(R,N,NP, I NDX , B) 
D I HENS I ON fi(NP , NP ) , INDX(N),B(N) 

I I -0 

DO 12 1-1 ,N 
LL= INDX( I ) 

SUtl-B(LL) 

B(LL)«B( I ) 

IF (II .HE .0) THEN 
DO 11 J-l I ,1-1 

sun«sun-R( i , j)*b( j) 

11 CONTINUE 

ELSE IF (SUn.NE.O. ) THEN 
I l-l 
END IF 
B( I )-sun 

12 CONTINUE 

DO M l-N, 1 ,-1 
sun«B( i ) 

I F ( I . LT . N )THEN 
DO 13 J=l + 1 ,N 

sun-sut1-fl( I , J)*B( J) 


13 

CONTINUE 


END IF 


B( 1 )-SUI1/n( 1,1) 

M 

CONTINUE 


RETURN 


END 


-j 
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USRFUN .FOR 

subroutine usrfun (r, alpha, beta) 


include 1 roo t . i nc ' 

real alpha(4,1), beta(4), r(4) 

calculate functions and derivatives fron I ae of cosines 

beta(1) - -( r( 1 )*r ( 1 ) + r(2)*r(2) - I 1-2 

e 2*r(1 )*r(2)*c( 1 ,2) - t ( 1 , 2 )*t ( 1 , 2) ) 

alpha( 1 , 1 ) - 2*r( 1 ) - 2*c( 1 , 2) *r ( 2) 
a I pha( 1 , 2 ) - 2*r(2) - 2* c ( 1 , 2 ) *r ( 1 ) 
a I pha( 1 , 3 ) = 0 
a I pha( 1 , 4 ) ■ 0 

bet a(2 ) - -( r(1)*r(1) + r(3)*r(3) - ! 1-3 

* 2*r(1 )*r(3)*c(1 ,3) - t ( 1 , 3 )«t ( 1 f 3) ) 

alpha(2,2) = 0 

a I pha( 2, 3 ) - 2*r(3) - 2*c( 1 , 3) *r ( 1 ) 
alpha(2,1) * 2 * r ( 1 ) - 2*c(1,3)*r3 
alpha(2,4) » 0 

bet a(3 ) ■ -( r(3)*r(3) + r(4)*r(4) - ! 3-4 

t 2*r (3 )*r (4) *c( 3 , 4 ) - t ( 3, 4 )*t (3, 4) ) 

a I pha( 3, 3 ) - 2*r(3) -2*c (3, 4)*r(4 ) 
a I pha( 3, 4 ) ■ 2*r(4) -2*c (3 , 4)*r (3 ) 
a I pha( 3, 1 ) = 0 
a I pha( 3, 2 ) 

bet a ( 4 ) - -( r(4)*r<4) + r(2)*r(2) - I 2-4 

« 2*r (4 )*r ( 1 ) *c( 2, 4 ) - t ( 2, 4 )*t (2, 4) ) 

a I pha( 4, 2 ) - 2*r(2) -2*c (2, 4)*r(4 ) 
a I pha( 4 , 4 ) - 2*r(4) -2*c (2, 4)*r(2) 
a lpha( 4, 1 ) ■ 0 
a I pha ( 4 , 3 ) - 0 

betasu* = 0 1 Convergence 

»rite(6,*) ’ r-> (r(i), i=1, 4) 

■rite(6,*) ‘beta-> (beta(i), 1=1, 4) 


ret urn 
end 



